//*****************************************************************
// Controller.cpp
//
//  Created on: Jan 21, 2012
//      Author: Christopher Wood
//              Robert Livosi
//
//  $Id: Controller.cpp 32 2012-01-30 04:49:23Z caw4567@gmail.com $
//*****************************************************************

#include "Controller.h"
#include "PIDControlAlgorithm.h"

/**
 * The (friendly) run function for the controller thread.
 *
 * @param arg - a pointer to the host Controller object.
 */
void* controllerThreadRun(void* arg);

/**
 * Create a new controller thread that will manage the discrete-time transformations.
 *
 * @param algorithmType - the type of transform/control algorithm to be used.
 */
Controller::Controller(AlgorithmType algorithmType)
{
	// Initialize the reference (set point).
	this->reference = 0;
	this->waitTick = 0;

	// Create the input/output execution semaphores
	sem_init(&controlSem, 0, 0);
	sem_init(&updateSem, 0, 1);

	// Finally, create the control algorithm
	switch (algorithmType)
	{
	case TRANSFORM_ALGORITHM_PID:
		algorithm = new PIDControlAlgorithm();
		break;
	default:
		cerr << "Error: Invalid control algorithm specified." << endl;
		break;
	}
}

/**
 * Default destructor.
 */
Controller::~Controller()
{
}

/**
 * Start the thread.
 */
void Controller::run()
{
	setThreadState(true);
	pthread_create(&controlThreadID, NULL, controllerThreadRun, this);
}

/**
 * The (friendly) run function for the controller thread.
 *
 * @param arg - a pointer to the host Controller object.
 */
void* controllerThreadRun(void* arg)
{
	Controller* controller = (Controller*)arg;
	float input = 0.0;
	float transform = 0.0;

	while (controller->isAlive())
	{
		// Wait until we have data to process
		sem_wait(controller->getControlSem());
		if (!controller->isAlive())
		{
			break;
		}

		// Check to see if we are settling down.
		if (controller->getWaitTick() == 0)
		{
			// Fetch the plant output and calculate the transform
			input = controller->getIOGroup()->getInputValue();
			sem_wait(controller->getUpdateSemaphore());
			transform = controller->getAlgorithm()->computeTransform(controller->getReference(), input);
			sem_post(controller->getUpdateSemaphore());
		}
		else
		{
			transform = controller->getReference();
			controller->decrementWaitTick();
		}

		// Send the transform the the output thread
		controller->getIOGroup()->setOutputValue(transform);
		sem_post(controller->getIOGroup()->getOutputThreadSem());
	}

	return NULL;
}

/**
 * Set the new reference value (set point).
 *
 * @param ref - the new reference value.
 */
void Controller::setReference(float ref)
{
	this->reference = ref;
}

/**
 * Retrieve the reference parameter.
 *
 * @return reference;
 */
float Controller::getReference()
{
	return reference;
}

/**
 * Set the IO group that is used to drive this controller.
 *
 * @param ioGroup - the desired ioGroup object (a pointer to it)
 */
void Controller::setIOGroup(IOThreadGroup* ioGroup)
{
	this->ioGroup = ioGroup;
}

/**
 * Retrieve the I/O thread group associated with this transform object.
 *
 * @return a pointer to the I/O thread group.
 */
IOThreadGroup* Controller::getIOGroup()
{
	return ioGroup;
}

/**
 * Retrieve the current control algorithm being used.
 *
 * @return a pointer to the current control algorithm.
 */
ControlAlgorithm* Controller::getAlgorithm()
{
	return algorithm;
}

/**
 * Retrieve the control execution semaphore used to trigger a conversion.
 *
 * @return a pointer to the control semaphore.
 */
sem_t* Controller::getControlSem()
{
	return &controlSem;
}

/**
 * Set the thread state of this transform thread.
 *
 * @param alive - boolean flag indicating the alive state of this thread.
 */
void Controller::setThreadState(bool alive)
{
	this->alive = alive;
}

/**
 * Release this thread from its semaphore.
 */
void Controller::release()
{
	sem_post(&controlSem);
}

/**
 * Determine if this thread is still alive (or should still be alive)
 *
 * @return true if still alive, false otherwise.
 */
bool Controller::isAlive()
{
	return alive;
}

/**
 * Method that is used to prompt the user for the
 * new algorithm parameters.
 *
 * @return a string for the user to interpret.
 */
string Controller::getParamString()
{
	return "Enter the set point: ";
}

/**
 * Set the parameters of the algorithm.
 *
 * @param params - string of new parameters
 * @returns true if parsing was successful, false otherwise.
 */
bool Controller::setParams(string params)
{
	bool successful = true;
	float newRef = 0.0;

	// Try to perform the string to float conversion
	try
	{
		newRef = atof(params.c_str());
	}
	catch (...)
	{
		successful = false;
	}

	// Only update if parsing was successful
	if (successful)
	{
		sem_wait(&updateSem);
		this->reference = newRef;
		sem_post(&updateSem);
	}

	return successful;
}

/**
 * Retrieve the update semaphore.
 *
 * @return update semaphore.
 */
sem_t* Controller::getUpdateSemaphore()
{
	return &updateSem;
}

/**
 * Reset the controller and control algorithm.
 */
void Controller::reset()
{
	this->reference = 0.0;
	waitTick = RESET_CYCLES;
	algorithm->reset();
}

/**
 * Retrieve the controller wait tick.
 *
 * @return waitTick
 */
uint8_t Controller::getWaitTick()
{
	return waitTick;
}

/**
 * Decrement the wait tick for the controller.
 */
void Controller::decrementWaitTick()
{
	waitTick--;
}
